#include "commands.h"
#include "../include/uart.h"
#include "../include/log.h"
#include "../include/type.h"
#include <stdbool.h>
#include <unistd.h>


// 控制模式和设备状态相关的全局变量定义

Motor_Ctr Speed_ClosedLoop_ctr(uint32_t speed) 
{
    Motor_Ctr rs485_frame = {0};
    rs485_frame.frame_header = 0x3E;
    rs485_frame.id = 0x01;
    rs485_frame.length = 0x08;
    rs485_frame.cmd = SPEED_CLOSEDLOOP_CTR;
    rs485_frame.null = 0x00;
    uint32_t speedControl = speed * 100;  // 转换为适当单位
    rs485_frame.data[0] = 0x00;
    rs485_frame.data[1] = 0x00;
    rs485_frame.data[2] = (uint8_t)speedControl;
    rs485_frame.data[3] = (uint8_t)(speedControl >> 8);
    rs485_frame.data[4] = (uint8_t)(speedControl >> 16);
    rs485_frame.data[5] = (uint8_t)(speedControl >> 24);

    LOG_DBG("Speed control frame created with speed: %d", speed);

    return rs485_frame;
}

// 设置电机速度
void set_motor_speed(int speed)
{
    if (speed > MOTOR_SPEED_MAX) {
        LOG_ERR("Invalid motor speed: %u", speed);
        return;
    }
    motor_speed=speed;
    host_ctr_cmd_g.motor_ctr=Speed_ClosedLoop_ctr(motor_speed);

    LOG_INFO("Motor speed set to %d", motor_speed);

    if(trans_flag){
        uart_send_command(&host_ctr_cmd_g, sizeof(host_ctr_cmd_g));  // 发送数据
    }
}

// 设置步进电机距离
void set_stepmotor_distance(float distance) {
    host_ctr_cmd_g.stepmotor_ctr.dist = distance;
    LOG_INFO("Step motor distance set to %.2f", distance);

    if (trans_flag) {
        uart_send_command(&host_ctr_cmd_g, sizeof(host_ctr_cmd_g));  // 发送数据
    }
}

// 发送电源开关命令
void set_power(bool power_on) {
    host_ctr_cmd_g.cmd = power_on ? POWER_ON : POWER_OFF;
    LOG_INFO("Power %s command sent", power_on ? "ON" : "OFF");

    uart_send_command(&host_ctr_cmd_g, sizeof(host_ctr_cmd_g));  // 发送数据

    if (!power_on) {
        host_ctr_cmd_g.cmd = SHELL_OPEN;  // 关闭通路并打开 shell,进入某种 Shell 模式，允许用户通过 Shell 命令来与设备交互。
        uart_send_command(&host_ctr_cmd_g, sizeof(host_ctr_cmd_g));
    }
}

